% main function
% kneed biped, AMBER model, with torso

%angle convention
%q0 - angle the stance calf makes with the vertical, counterclockwise is positive
%q1 - relative stance knee angle,  counterclockwise is positive
%q2 - relative hip angle,          clockwise is positive
%q3 - relative swing knee angle,   counterclockwise is positive
% at q = (0,0,0,0) the biped is standing straight up, with locked knees

% clc;clear all;
function main_AMBER_No(OutputType)
clear tp tp0 yact ydes p data t u U tp Pow
% addpath('./build_torso')
model_folder_name =['./model/build_torso_' OutputType]; 
addpath(model_folder_name)
addpath('./wrappers_torso')
addpath('./result')
%Simulation variables
g = 981/100;
numsteps =10;
tf = 0;
data = cell(numsteps,1);

% Parameters
umax = 8.9;

%From numeric optimization

% typeofwalking = 3  %%%%%%% SHU
typeofwalking = 2

if typeofwalking == 1
    
    load('a_temp_good');
    a = a_temp;
    load('ic')
    ic = ic;
    
elseif typeofwalking == 2
    
% %     load('a_opt');
% %     load('x_opt');

    load('a_opt');
    load('x_opt');
    ic = x_opt;%+[0;0;pi;pi;0;0;0;0;0;0];
%     ic = zeros(size(x_opt));
    a = a_opt;
%     a = [
%     0.1920         0         0         0         0
%     3.4097   12.0643    2.4901    3.9575   -1.3693
%     2.2941    2.5816    3.7726    1.1892   -0.0105
%    -2.3698   -0.7459   -2.0779   -1.4069    1.9145
%     1.7981    1.2937   -3.5077    1.4972   -0.3934];
%     
elseif typeofwalking == 3
    
    load('a_temp');
    a = a_temp;
    load('x_temp')
    ic = x_temp;
    
    
end

ep = 20; %a(1,5);



%%%Variables used to store output data
global tp tp0 yact ydes p U umax Pow

tp0 = 0;
tp = [];
yact = [];
ydes = [];
U = [];
Pow = [];

%%%%%%(Comment this line if your initial condition is post-impact%%%%%
ic = resetFunc(ic);
ndof = length(ic)/2;

%Initial position of the hip

p = phip_sca(ic);



%Initialize animation plot
figure(2);
clf
ramp = plot([-1 2], [0 0], 'b:');
px=0;
py=0;
hold on;
global legs;
legs = plot([0 0], 'k');
hold off;
xlim([-1 10]);
axis equal

        
%plot initial condition
% pos = jpos_mat(ic);
%         phipz = pos(1,3)-p(1)-px;
%         set(legs, 'XData', pos(1,:), 'YData', pos(2,:), ...
%             'erasemode', 'normal');
%         axis equal;
%         hold on;
%         drawnow;
        
%         pause(1000);

%%
for i=1:numsteps
    sol = ode45(@(t,x) f1_vector(t,x,a,ep), [0 1], ic, ...
        odeset('Events', @(t,x) eventfcn(t,x,a), 'MaxStep', 1e-2));
    
    T = tf + sol.x;
    data{i} = [T; sol.y];
    ictemp = sol.y(:,end);
    ic = resetFunc(sol.y(:,end));
    tf = tf + sol.x(end);
    tp0 = tf;
    
    F = [sol.x; sol.y];
    [m, k] = size(data{i});    
    qo = cat(2, data{:});
    
    %
    for n=1:k
        % position
        x = [px;py;sol.y(:,n)];
        njoints = length(jpos_mat(sol.y(1:ndof,n)));
        pos = jpos_mat(sol.y(1:ndof,n))+...
            [px*ones(1,njoints); py*ones(1,njoints)];
        phipz = pos(1,3)-p(1)-px;
        set(legs, 'XData', pos(1,:), 'YData', pos(2,:), ...
            'erasemode', 'normal');
        axis equal;
        hold on;
        drawnow;
%          pause(0.3);
        
    end
    %update positions
    px = pos(1,end);
    py = pos(2,end);
    %update initial position of the hip
%     ph = pos(1,3)-px; %%%%%%%%%%%%%%%%%%%%% fix hip ic position
%     p = ph; %%%%%%%%%%%%%%%%%%%%% fix hip ic position
end
figname = strcat('result/',OutputType,'Config.pdf');
saveas(gcf,figname)

%%


q=cat(2,data{:});

fig1 = figure(1); clf;
subplot(2,1,1);
plot(q(1,:),q(2:6,:));
legend('Location','EastOutside',{'\theta_{sf}','\theta_{sk}','\theta_{ship}','\theta_{nship}','\theta_{nsk}'});
subplot(2,1,2);
plot(q(2,:),q(7,:), q(3,:),q(8,:), q(4,:),q(9,:),q(5,:),q(10,:),q(6,:),q(11,:));
legend('Location','EastOutside',{'\theta_{sf}','\theta_{sk}','\theta_{ship}','\theta_{nship}','\theta_{nsk}'});
figname = strcat('result/',OutputType,'LC.pdf');
saveas(fig1,figname)


for i = 1:length(q)
    t(i) = q(1,i);
    g(i) = h_sca(q(2:end,i));
end

fig4=figure(4); clf;
plot(tp,yact,'k',tp,ydes,'r',t,g,'g')
title('ya vs yd')
xlabel('time (s)');
ylabel('desired vs. actual')
legend('Location','EastOutside',{'y_1 actual','y_2 actual','y_3 actual','y_4 actual','y_5 actual','y_1 desired','y_2 desired','y_3 desired','y_4 desired','y_5 desired','guard'});
figname = strcat('result/',OutputType,'yaVSyd.pdf');
saveas(fig4,figname)

% close all;
%Must be only over one step to be accurate

%%
% 2) Limit the average torque over a step to 1.95 N-m ( this is to limit heat dissipation through winding resistance).
% 3) Limit the average power over a step to 8.61 W (this is to limit overall heat dissipation)
% 4) Limit the maximum torque to 8.9 N-m (the motor cannot provide more than this).
% 5) Limit the maximum speed to 6.54 rad/s ( the brushes will not connect well beyond this speed)

% clear tstep qstep
% qstep = data{1};
% tstep = qstep(1,:);
% [scotu,scotp,Ustep] = scot(qstep,a);
% 
% fig6=figure(6); clf;
% % subplot(2,1,1);
% % plot(tp,U,tp,AvgU*ones(1,length(tp)),'ko',tp,AvgP*ones(1,length(tp)),'ks')
% plot(tstep,Ustep,...
%     tstep,8.9*ones(1,length(tstep)),'k+',tstep,-8.9*ones(1,length(tstep)),'k+',...
%     tstep,scotu*ones(1,length(tstep)),'m+',tstep,scotp*ones(1,length(tstep)),'m-')
% title('Torque Outputs')
% xlabel('time (s)');
% ylabel('Torque (NM)')
% legend('Location','EastOutside',{'\theta_{sf} torque','\theta_{sk} torque','\theta_{ship} torque','\theta_{nship} torque','\theta_{nsk} torque',...
%     'max torque','min torque','u^2 norm','SCOT'});
% figname = strcat(OutPutType,'TorqueOutputs.pdf');
% saveas(fig6,figname)
% subplot(2,1,2);
% plot(tp,Pow,tp,AvgP*ones(1,length(tp)))
% title('Power Outputs')
% xlabel('time (s)');
% ylabel('Power')
% legend('Location','EastOutside',{'\theta_{sf} power','\theta_{sk} power','\theta_{hip} power','\theta_{nsk} power'});

%%

%%Output data to be tracked
%%%

% % % outputdata = 1
% % % 
% % % if outputdata == 1
% % % 
% % % 
% % % 
% % % qtemp = data{1};
% % % %!!! CHANGE - Dropping last data point
% % % timeShift = qtemp(1, end) - qtemp(1, 1);
% % % qtime = [qtemp(1, 1:(end - 1)), timeShift + qtemp(1, :)];
% % % qknee_left = [qtemp(3, 1:(end - 1)),qtemp(6,:)];
% % % qknee_right = [qtemp(6,1:(end - 1)),qtemp(3,:)];
% % % qhip_left = [qtemp(4,1:(end - 1)),qtemp(5,:)];
% % % qhip_right = [qtemp(5,1:(end - 1)),qtemp(4,:)];
% % % %{
% % % qankle_stance = qtemp(2,:);
% % % qankle_nstance = repmat(qtemp(1,end), size(qankle_stance));
% % % %}
% % % qankle_stance = qtemp(2,:);
% % % % Will try removing q(3), or qtemp(4, :) since torso is not actuated in
% % % % NAO
% % % qankle_nstance = qtemp(2, :) + qtemp(3, :) + qtemp(4, :) - (qtemp(5, :) + qtemp(6, :));
% % % qankle_left = [qankle_stance(1:(end - 1)), qankle_nstance];
% % % qankle_right = [qankle_nstance(1:(end - 1)), qankle_stance];
% % % % qkneevel = [qtemp(9,:),qtemp(7,:)];
% % % % qkneetorq = [U(4,:),U(2,:)];
% % % 
% % % figure(8); clf;
% % % plot(qtime,qknee_left,qtime,qhip_left,qtime,qknee_right,qtime,qhip_right)
% % % legend('Location','EastOutside',{'Left knee','Left hip','Right knee','Right hip'})
% % % 
% % % % 
% % % % figure(7); clf;
% % % % plot(qtime,qknee,qtime,qkneevel,[tp, tp+tp(end)],qkneetorq)
% % % % title('Knee behavior')
% % % % xlabel('time (s)');
% % % % legend('Location','EastOutside',{'position','velocity','torque'})
% % % 
% % % addpath('./ExperimentData')
% % % save('./ExperimentData/kneetraj.mat','qtime','qknee_left','qhip_left','qhip_right','qknee_right','qankle_left','qankle_right')
% % % save ./ExperimentData/knee_left_traj qknee_left -ascii -double -tabs
% % % save ./ExperimentData/hip_left_traj qhip_left -ascii -double -tabs
% % % save ./ExperimentData/knee_right_traj qknee_right -ascii -double -tabs
% % % save ./ExperimentData/hip_right_traj qhip_right -ascii -double -tabs
% % % 
% % % end
